//------------------------------------------------------------------------------
//
//  Generic Brick Test
//
//  Written Jan-2008 by Trevor Taylor, Software Technology, Australia
//
//  This is a command-line service for testing the operations of a robot
//  service that is based on the Generic Brick contract.
//
//  This service is NOT intended to be used by other services, so it really
//  does not need a Proxy.
//
//  The manifest must include a Generic Brick robot. See the manifest for
//  some examples.
//
//  NOTE: If an operation is not implemented, it will probably throw an
//  exception. If you are running this service in the debugger, it will
//  break. You can simply continue to see the Fault. However, if you are
//  not running with the debugger you will see a Fault message immediately.
//
//------------------------------------------------------------------------------

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Xml;
using W3C.Soap;
using genericbricktest = ProMRDS.Robotics.GenericBrickTest;


using brick = ProMRDS.Robotics.GenericBrick.Proxy;
using drive = Microsoft.Robotics.Services.Drive.Proxy;
using util = ProMRDS.Robotics.GenericBrickUtil.Util;

using Microsoft.Dss.Core.DsspHttp;


namespace ProMRDS.Robotics.GenericBrickTest
{
    /// <summary>
    /// Implementation class for GenericBrickTest
    /// </summary>
    [DisplayName("GenericBrickTest")]
    [Description("The GenericBrickTest Service")]
    [Contract(Contract.Identifier)]
    public class GenericBrickTestService : DsspServiceBase
    {
        
        /// <summary>
        /// Generic Brick Test State
        /// </summary>
        private GenericBrickTestState _state = new GenericBrickTestState();
        
        /// <summary>
        /// Generic Brick Test Main Port
        /// </summary>
        [ServicePort("/genericbricktest", AllowMultipleInstances=false)]
        private GenericBrickTestOperations _mainPort = new GenericBrickTestOperations();


        // Generic Brick Partner (or any robot based on it)
        [Partner("Brick", Contract = brick.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
        private brick.GenericBrickOperations _brickPort = new brick.GenericBrickOperations();
        private brick.GenericBrickOperations _notifyPort = new brick.GenericBrickOperations();

        // IMPORTANT NOTE:
        // These operations must be kept in sync with the GenericBrick contract.
        // It should probably be done by reflection, but that is not as easy as
        // it sounds. In particular, the various test routines have some knowledge
        // of the type of operation that they are executing so that they do not do
        // anything stupid to the robot! That is not something that you can figure
        // out using reflection.
        private string[] Operations =
        {
                "DsspDefaultLookup",    // 0
                "DsspDefaultDrop",      // 1
                "Get",                  // 2
                "HttpGet",              // 3
                "HttpPost",             // 4
                "Replace",              // 5 Replaces entire state
                "Subscribe",            // 6 Subscribe to changes
                "ConfigureBrick",       // 7 Changes Brick Configuration
                "ConfigureDevices",     // 8 Changes Devices (if possible)
                "UpdateSensors",        // 9 Selective update / notification
                "PlayTone",             // 10 Play a Tone
                "SetLEDs",              // 11 Turn LEDs on/off
                "GetSwitches",          // 12 Read all switches
                "GetSensors",           // 13 Get list of Digital and Analog Inputs
                "SetActuators",         // 14 Set list of Digital and Analog Outputs
                "QueryDrivePower",      // 15 Query current Drive Power
                "drive.SetDrivePower",  // 16 Update power to motors
                "drive.DriveDistance",  // 17 Drive a specified distance
                "drive.RotateDegrees"   // 18 Rotate a specified number of degrees
        };

        // Port to flag completion of operations
        Port<bool> _completionPort = new Port<bool>();

        // Various global flags
        bool _gotState = false;
        bool _subscribed = false;
        bool _showNotification = true;


        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public GenericBrickTestService(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
        }
        
        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
            // Initialize the state
            if (_state.Status == null)
                _state.Status = new bool[Operations.Length];
            if (_state.BrickState == null)
                _state.BrickState = new brick.GenericBrickState();

			base.Start();

            MainPortInterleave.CombineWith(
                new Interleave(
                    new ExclusiveReceiverGroup(
                        Arbiter.ReceiveWithIterator<brick.UpdateSensors>(true, _notifyPort, SubscriptionHandler)
                    ),
                    new ConcurrentReceiverGroup(
                    )
                )
            );

            // Start the main loop as an Iterator
            SpawnIterator(MainLoop);
        }

        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }

        private IEnumerator<ITask> MainLoop()
        {
            bool done = false;
            bool skipWait;
            string s;
            int num = 0;

            while (!done)
            {
                skipWait = false;

                // Display the list of available operations
                Console.ForegroundColor = ConsoleColor.Green;
                Console.WriteLine("Select from the following:");
                Console.WriteLine("0 = Exit");
                for (int i = 1; i <= Operations.Length; i++)
                {
                    Console.WriteLine(i + " = " + Operations[i - 1]);
                }

                // Ask user for a selection
                Console.ResetColor();
                s = Console.ReadLine();

                // Clear the completion port so we do not get out of synch
                // if responses are delayed (only happens on an Exception)
                _completionPort.Clear();

                try
                {
                    // Try to get an integer from the user's input
                    num = int.Parse(s);
                    // Execute the selected example
                    switch (num)
                    {
                        case 0:
                            done = true;
                            break;
                        case 1:
                            SpawnIterator(DoLookup);
                            break;
                        case 2:
                            SpawnIterator(DoDrop);
                            break;
                        case 3:
                            SpawnIterator(DoGet);
                            break;
                        case 4:
                            SpawnIterator(DoHttpGet);
                            break;
                        case 5:
                            SpawnIterator(DoHttpPost);
                            break;
                        case 6:
                            SpawnIterator(DoReplace);
                            break;
                        case 7:
                            SpawnIterator(DoSubscribe);
                            break;
                        case 8:
                            SpawnIterator(DoConfigureBrick);
                            break;
                        case 9:
                            SpawnIterator(DoConfigureDevices);
                            break;
                        case 10:
                            SpawnIterator(DoUpdateSensors);
                            break;
                        case 11:
                            SpawnIterator(DoPlayTone);
                            break;
                        case 12:
                            SpawnIterator(DoSetLEDs);
                            break;
                        case 13:
                            SpawnIterator(DoGetSwitches);
                            break;
                        case 14:
                            SpawnIterator(DoGetSensors);
                            break;
                        case 15:
                            SpawnIterator(DoSetActuators);
                            break;
                        case 16:
                            SpawnIterator(DoQueryDrivePower);
                            break;
                        case 17:
                            SpawnIterator(DoSetDrivePower);
                            break;
                        case 18:
                            SpawnIterator(DoDriveDistance);
                            break;
                        case 19:
                            SpawnIterator(DoRotateDegrees);
                            break;
                        default:
                            Console.ForegroundColor = ConsoleColor.Red;
                            Console.WriteLine("Not a valid selection");
                            Console.ResetColor();
                            skipWait = true;
                            break;
                    }
                }
                catch
                {
                    // User entered something, but it was not a number
                    Console.ForegroundColor = ConsoleColor.Red;
                    Console.WriteLine("Please enter only numbers");
                    Console.ResetColor();
                    skipWait = true;
                }

                if (done)
                {
                    // Exit
                    ControlPanelPort.Shutdown();
                    yield break;
                }

                if (!skipWait)
                {
                    // Wait for completion but with a timeout
                    yield return Arbiter.Choice(
                        Arbiter.Receive<bool>(false, _completionPort,
                        delegate(bool status)
                        {
                            // Set a flag for this operation to say that it has
                            // executed successfully. The user can look in the
                            // service state to see which ones worked.
                            _state.Status[num - 1] = status;
                        }
                        ),
                        Arbiter.Receive<DateTime>(false, TimeoutPort(5000),
                        delegate(DateTime dt)
                        {
                            LogError(LogGroups.Console, "Operation timed out");
                        }
                        )
                    );

                    Console.ForegroundColor = ConsoleColor.Yellow;
                    Console.Write("Press Enter to continue:");
                    s = Console.ReadLine();
                }
            }
        }

        // NOTE:
        // The various operation tests follow a standard format:
        // 1. Execute the operation
        // 2. Display the result (or as much of it as is reasonable)
        //    OR
        //    Display the Fault
        // 3. Post back the status

        private IEnumerator<ITask> DoLookup()
        {
            bool status = true;

            yield return Arbiter.Choice(
                _brickPort.DsspDefaultLookup(),
                delegate(LookupResponse resp)
                {
                    LogInfo(LogGroups.Console,
                        "Lookup Response\n" +
                        "Contract = " + resp.Contract + "\n" +
                        "Service = " + resp.Service + "\n" +
                        "HttpUri = " + resp.HttpUri() + "\n");
                    status = true;
                },
                delegate(Fault f)
                {
                    LogError(LogGroups.Console,
                        "Lookup Fault\n" +
                        FormatFault(f));
                    status = false;
                }
            );

            // Post back the status so the main loop can continue
            _completionPort.Post(status);            
        }

        private IEnumerator<ITask> DoDrop()
        {
            bool status = true;

            yield return Arbiter.Choice(
                _brickPort.DsspDefaultDrop(),
                delegate(DefaultDropResponseType resp)
                {
                    LogInfo(LogGroups.Console,
                        "Drop Response received\n");
                    status = true;
                },
                delegate(Fault f)
                {
                    LogError(LogGroups.Console,
                        "Drop Fault\n" +
                        FormatFault(f));
                    status = false;
                }
            );

            // Post back the status so the main loop can continue
            _completionPort.Post(status);
        }

        private IEnumerator<ITask> DoGet()
        {
            bool status = true;

            yield return Arbiter.Choice(
                _brickPort.Get(),
                delegate(brick.GenericBrickState resp)
                {
                    LogInfo(LogGroups.Console,
                        "Get Response received (check in web browser)\n");
                    status = true;
                    _state.BrickState = resp;
                    _gotState = true;
                },
                delegate(Fault f)
                {
                    LogError(LogGroups.Console,
                        "Get Fault\n" +
                        FormatFault(f));
                    status = false;
                }
            );

            // Post back the status so the main loop can continue
            _completionPort.Post(status);
        }

        private IEnumerator<ITask> DoHttpGet()
        {
            bool status = true;

            LogInfo(LogGroups.Console, "You should use a web browser to check this operation");

            yield return Arbiter.Choice(
                _brickPort.HttpGet(),
                delegate(HttpResponseType resp)
                {
                    LogInfo(LogGroups.Console,
                        "HtpGet Response received\n");
                    status = true;
                },
                delegate(Fault f)
                {
                    LogError(LogGroups.Console,
                        "HttpGet Fault\n" +
                        FormatFault(f));
                    status = false;
                }
            );

            // Post back the status so the main loop can continue
            _completionPort.Post(status);
        }

        private IEnumerator<ITask> DoHttpPost()
        {
            bool status = true;

            LogInfo(LogGroups.Console, "You should use a web browser to check this operation");

            yield return Arbiter.Choice(
                _brickPort.HttpPost(),
                delegate(HttpResponseType resp)
                {
                    LogInfo(LogGroups.Console,
                        "HtpPost Response received\n");
                    status = true;
                },
                delegate(Fault f)
                {
                    LogError(LogGroups.Console,
                        "HttpPost Fault\n" +
                        FormatFault(f));
                    status = false;
                }
            );

            // Post back the status so the main loop can continue
            _completionPort.Post(status);
        }

        private IEnumerator<ITask> DoReplace()
        {
            bool status = true;

            if (!_gotState)
            {
                LogInfo(LogGroups.Console, "Please do a Get first");
                _completionPort.Post(false);
                yield break;
            }

            yield return Arbiter.Choice(
                _brickPort.Replace(_state.BrickState),
                delegate(DefaultReplaceResponseType resp)
                {
                    LogInfo(LogGroups.Console,
                        "Replace Response received\n");
                    status = true;
                },
                delegate(Fault f)
                {
                    LogError(LogGroups.Console,
                        "Replace Fault\n" +
                        FormatFault(f));
                    status = false;
                }
            );

            // Post back the status so the main loop can continue
            _completionPort.Post(status);
        }

        private IEnumerator<ITask> DoSubscribe()
        {
            bool status = true;

            if (_subscribed)
            {
                LogInfo(LogGroups.Console, "Already subscribed");
                _completionPort.Post(false);
                yield break;
            }

            yield return Arbiter.Choice(
                _brickPort.Subscribe(_notifyPort),
                delegate(SubscribeResponseType resp)
                {
                    LogInfo(LogGroups.Console,
                        "Subscribe Response received\n" +
                        "Subscriber: " + resp.Subscriber + "\n" +
                        "Subscription Manager: " + resp.SubscriptionManager + "\n");
                    status = true;
                    _subscribed = true;
                },
                delegate(Fault f)
                {
                    LogError(LogGroups.Console,
                        "Subscribe Fault\n" +
                        FormatFault(f));
                    status = false;
                }
            );

            // Post back the status so the main loop can continue
            _completionPort.Post(status);
        }

        private IEnumerator<ITask> DoConfigureBrick()
        {
            bool status = true;

            if (!_gotState)
            {
                LogInfo(LogGroups.Console, "Please do a Get first");
                _completionPort.Post(false);
                yield break;
            }

            yield return Arbiter.Choice(
                _brickPort.ConfigureBrick(_state.BrickState.Configuration),
                delegate(DefaultUpdateResponseType resp)
                {
                    LogInfo(LogGroups.Console,
                        "ConfigureBrick Response received\n");
                    status = true;
                },
                delegate(Fault f)
                {
                    LogError(LogGroups.Console,
                        "ConfigureBrick Fault\n" +
                        FormatFault(f));
                    status = false;
                }
            );

            // Post back the status so the main loop can continue
            _completionPort.Post(status);
        }

        private IEnumerator<ITask> DoConfigureDevices()
        {
            bool status = true;
            List<brick.Device> devices = new List<brick.Device>();

            if (!_gotState)
            {
                LogInfo(LogGroups.Console, "Please do a Get first");
                _completionPort.Post(false);
                yield break;
            }

            for (int i = 0; i < _state.BrickState.Sensors.Count; i++)
            {
                if ((_state.BrickState.Sensors[i].Type & brick.DeviceTypes.Reconfigurable) != 0)
                {
                    devices.Add(_state.BrickState.Sensors[i]);
                }
            }

            if (devices.Count == 0)
            {
                LogError(LogGroups.Console, "No reconfigurable devices found");
                _completionPort.Post(false);
                yield break;
            }

            yield return Arbiter.Choice(
                _brickPort.ConfigureDevices(devices),
                delegate(DefaultUpdateResponseType resp)
                {
                    LogInfo(LogGroups.Console,
                        "ConfigureDevices Response received\n");
                    status = true;
                },
                delegate(Fault f)
                {
                    LogError(LogGroups.Console,
                        "ConfigureDevices Fault\n" +
                        FormatFault(f));
                    status = false;
                }
            );

            // Post back the status so the main loop can continue
            _completionPort.Post(status);
        }

        private IEnumerator<ITask> DoUpdateSensors()
        {
            bool status = true;

            if (!_gotState)
            {
                LogInfo(LogGroups.Console, "Please do a Get first");
                _completionPort.Post(false);
                yield break;
            }

            List<brick.Device> sensors = new List<brick.Device>();

            // Copy only the sensors that are NOT reserved
            // and that are Digital or Analog devices
            for (int i = 0; i < _state.BrickState.Sensors.Count; i++)
            {
                if ((_state.BrickState.Sensors[i].Type & brick.DeviceTypes.Reserved) == 0)
                {
                    if (util.IsDigitalDevice(_state.BrickState.Sensors[i]) ||
                        util.IsAnalogDevice(_state.BrickState.Sensors[i]))
                    {
                        sensors.Add(_state.BrickState.Sensors[i]);
                    }
                }
            }
            DisplayDeviceList(sensors);

            yield return Arbiter.Choice(
                _brickPort.UpdateSensors(sensors),
                delegate(DefaultUpdateResponseType resp)
                {
                    LogInfo(LogGroups.Console,
                        "Update Sensors Response received\n");
                    status = true;
                },
                delegate(Fault f)
                {
                    LogError(LogGroups.Console,
                        "UpdateSensors Fault\n" +
                        FormatFault(f));
                    status = false;
                }
            );

            // Post back the status so the main loop can continue
            _completionPort.Post(status);
        }

        private IEnumerator<ITask> DoPlayTone()
        {
            bool status = true;

            yield return Arbiter.Choice(
                _brickPort.PlayTone(3000, 1000),
                delegate(DefaultUpdateResponseType resp)
                {
                    LogInfo(LogGroups.Console,
                        "PlayTone Response received\n");
                    status = true;
                },
                delegate(Fault f)
                {
                    LogError(LogGroups.Console,
                        "PlayTone Fault\n" +
                        FormatFault(f));
                    status = false;
                }
            );

            // Post back the status so the main loop can continue
            _completionPort.Post(status);
        }

        private IEnumerator<ITask> DoSetLEDs()
        {
            bool status = true;
            string s;
            int leds;

            Console.Write("Enter LED States (integer): ");
            s = Console.ReadLine();
            if (s == "")
                yield break;
            leds = int.Parse(s);

            yield return Arbiter.Choice(
                _brickPort.SetLEDs(leds),
                delegate(DefaultUpdateResponseType resp)
                {
                    LogInfo(LogGroups.Console,
                        "SetLEDs Response received\n");
                    status = true;
                },
                delegate(Fault f)
                {
                    LogError(LogGroups.Console,
                        "SetLEDs Fault\n" +
                        FormatFault(f));
                    status = false;
                }
            );

            // Post back the status so the main loop can continue
            _completionPort.Post(status);
        }

        private IEnumerator<ITask> DoGetSwitches()
        {
            bool status = true;

            yield return Arbiter.Choice(
                _brickPort.GetSwitches(),
                delegate(brick.GetSwitchesResponse resp)
                {
                    LogInfo(LogGroups.Console,
                        "GetSwitches Response\n" +
                        "States = " + IntToHex(resp.States) + "\n");
                    status = true;
                    _state.BrickState.Switches = resp.States;
                },
                delegate(Fault f)
                {
                    LogError(LogGroups.Console,
                        "GetSwitches Fault\n" +
                        FormatFault(f));
                    status = false;
                }
            );

            // Post back the status so the main loop can continue
            _completionPort.Post(status);
        }

        private IEnumerator<ITask> DoGetSensors()
        {
            bool status = true;

            if (!_gotState)
            {
                LogInfo(LogGroups.Console, "Please do a Get first");
                _completionPort.Post(false);
                yield break;
            }

            yield return Arbiter.Choice(
                _brickPort.GetSensors(_state.BrickState.Sensors),
                delegate(brick.GetSensorsResponse resp)
                {
                    LogInfo(LogGroups.Console,
                        "GetSensors Response received\n");
                    DisplayDeviceList(resp.Inputs);
                    status = true;
                    _state.BrickState.Sensors = resp.Inputs;
                },
                delegate(Fault f)
                {
                    LogError(LogGroups.Console,
                        "GetSensors Fault\n" +
                        FormatFault(f));
                    status = false;
                }
            );

            // Post back the status so the main loop can continue
            _completionPort.Post(status);
        }

        private IEnumerator<ITask> DoSetActuators()
        {
            bool status = true;

            if (!_gotState)
            {
                LogInfo(LogGroups.Console, "Please do a Get first");
                _completionPort.Post(false);
                yield break;
            }

            List<brick.Device> actuators = new List<brick.Device>();

            // Copy only the actuators that are NOT reserved
            // and that are Digital or Analog devices
            for (int i = 0; i < _state.BrickState.Actuators.Count; i++)
            {
                if ((_state.BrickState.Actuators[i].Type & brick.DeviceTypes.Reserved) == 0)
                {
                    if (util.IsDigitalDevice(_state.BrickState.Actuators[i]) ||
                        util.IsAnalogDevice(_state.BrickState.Actuators[i]))
                    {
                        actuators.Add(_state.BrickState.Actuators[i]);
                    }
                }
            }
            DisplayDeviceList(actuators);

            yield return Arbiter.Choice(
                _brickPort.SetActuators(actuators),
                delegate(DefaultUpdateResponseType resp)
                {
                    LogInfo(LogGroups.Console,
                        "SetActuators Response received\n");
                    status = true;
                },
                delegate(Fault f)
                {
                    LogError(LogGroups.Console,
                        "SetActuators Fault\n" +
                        FormatFault(f));
                    status = false;
                }
            );

            // Post back the status so the main loop can continue
            _completionPort.Post(status);
        }

        private IEnumerator<ITask> DoQueryDrivePower()
        {
            bool status = true;

            yield return Arbiter.Choice(
                _brickPort.QueryDrivePower(),
                delegate(brick.DrivePower resp)
                {
                    LogInfo(LogGroups.Console,
                        "QueryDrivePower Response\n" +
                        "Power = " + resp.LeftPower + "," + resp.RightPower + "\n");
                    status = true;
                    _state.BrickState.DrivePower = resp;
                },
                delegate(Fault f)
                {
                    LogError(LogGroups.Console,
                        "GetSwitches Fault\n" +
                        FormatFault(f));
                    status = false;
                }
            );

            // Post back the status so the main loop can continue
            _completionPort.Post(status);
        }

        private IEnumerator<ITask> DoSetDrivePower()
        {
            bool status = true;
            string s;
            double power;

            Console.Write("Enter Power: ");
            s = Console.ReadLine();
            if (s == "")
                yield break;
            power = double.Parse(s);

            yield return Arbiter.Choice(
                _brickPort.SetDrivePower(new drive.SetDrivePowerRequest(power, power)),
                delegate(DefaultUpdateResponseType resp)
                {
                    LogInfo(LogGroups.Console,
                        "SetDrivePower Response received\n");
                    status = true;
                },
                delegate(Fault f)
                {
                    LogError(LogGroups.Console,
                        "SetDrivePower Fault\n" +
                        FormatFault(f));
                    status = false;
                }
            );

            // Post back the status so the main loop can continue
            _completionPort.Post(status);
        }

        private IEnumerator<ITask> DoDriveDistance()
        {
            bool status = true;
            string s;
            double power = 0.5;
            double distance;

            Console.Write("Enter Distance (cm): ");
            s = Console.ReadLine();
            if (s == "")
                yield break;
            distance = double.Parse(s);
            distance /= 100;

            yield return Arbiter.Choice(
                _brickPort.DriveDistance(new drive.DriveDistanceRequest(distance, power)),
                delegate(DefaultUpdateResponseType resp)
                {
                    LogInfo(LogGroups.Console,
                        "DriveDistance Response received\n");
                    status = true;
                },
                delegate(Fault f)
                {
                    LogError(LogGroups.Console,
                        "DriveDistance Fault\n" +
                        FormatFault(f));
                    status = false;
                }
            );

            // Post back the status so the main loop can continue
            _completionPort.Post(status);
        }

        private IEnumerator<ITask> DoRotateDegrees()
        {
            bool status = true;
            string s;
            double power = 0.5;
            double angle;

            Console.Write("Enter Rotation Angle (degrees): ");
            s = Console.ReadLine();
            if (s == "")
                yield break;
            angle = double.Parse(s);

            yield return Arbiter.Choice(
                _brickPort.RotateDegrees(new drive.RotateDegreesRequest(angle, power)),
                delegate(DefaultUpdateResponseType resp)
                {
                    LogInfo(LogGroups.Console,
                        "RotateDegrees Response received\n");
                    status = true;
                },
                delegate(Fault f)
                {
                    LogError(LogGroups.Console,
                        "RotateDegrees Fault\n" +
                        FormatFault(f));
                    status = false;
                }
            );

            // Post back the status so the main loop can continue
            _completionPort.Post(status);
        }

        // Handle Notifications from Subscriptions
        private IEnumerator<ITask> SubscriptionHandler(brick.UpdateSensors update)
        {
            // The FIRST time, display the notification results,
            // then don't do it again because it will get REALLY annoying!
            if (_showNotification)
            {
                DisplayDeviceList(update.Body.Devices);
                _showNotification = false;
            }
            _state.NotificationCount++;
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        // Convert a number to hexadecimal
        private string IntToHex(int num)
        {
            return (string.Format("{0:X}", num));
        }

        // Format a Fault in a more presentable way
        private string FormatFault(Fault f)
        {
            string result = "";
            if (f.Code != null)
                result += "Code = " + f.Code + "\n";
            if (f.Node != null)
                result += "Node = " + f.Node + "\n";
            if (f.Role != null)
                result += "Role = " + f.Role + "\n";
            if (f.Reason != null)
            {
                for (int i=0; i<f.Reason.Length; i++)
                result += "Reason = " + f.Reason[i].Value + "\n";
            }
            if (f.Detail != null)
                result += "Detail = " + f.Detail.ToString() + "\n";
            if (result == "")
                return "No information in Fault";
            else
                return result;
        }


        // This is a much simplified display of a List of Devices
        // It uses Console.WriteLine because LogInfo puts too much
        // extra garbage around the data
        private void DisplayDeviceList(List<brick.Device> devices)
        {

            for (int i = 0; i < devices.Count; i++)
            {
                string devinfo;

                devinfo = devices[i].Name + " " +
                            devices[i].HardwareIdentifer + ",";
                if (util.IsAnalogDevice(devices[i]))
                {
                    devinfo += "Analog," + devices[i].Value;
                }
                else if (util.IsDigitalDevice(devices[i]))
                {
                    devinfo += "Digital," + devices[i].State;
                }
                else
                {
                    devinfo += "Unknown," + devices[i].Value;
                }

                Console.WriteLine("Device " + i + ": " + devinfo);
            }
        }

    }


}
